import time

from PySide2.QtGui import QColor, QPen
from PySide2.QtWidgets import QApplication, QGraphicsObject, QGraphicsView, QGraphicsScene, QGraphicsItem, \
    QGraphicsLineItem, QGraphicsTextItem
from PySide2.QtCore import Qt, QRectF, QPointF
from controller import geometric
import sys
import numpy as np
class Terminal2D(QGraphicsObject):
    def __init__(self,parent, sizeX=40,sizeY=20,posX=0,posY=0,name='头部',color=(100,255,100,160)):
        super().__init__()
        self.parent=parent
        self.sizeX = sizeX*1000
        self.sizeY=sizeY*1000
        self.position = QPointF(posX,posY)
        self.angle = 0.0
        self.name=name
        self.color=QColor(color[0],color[1],color[2],color[3])
        self.SetPos(posX,posY)
        self.setFlag(QGraphicsItem.ItemIsMovable)


    def SetPos(self,terminal:geometric.Terminal,view='z'):
        '''
        设置位置
        :param terminal:
        :param view:
        :return:
        '''
        if view=='x':
            self.position.setX(terminal.y*1000)
            self.position.setY(-terminal.z*1000)
            self.angle = -np.rad2deg(terminal.roll)
            self.setPos(self.position)
            self.setRotation(self.angle)
        if view=='y':
            self.position.setX(terminal.x*1000)
            self.position.setY(int(-terminal.z*1000))
            self.angle = np.rad2deg(terminal.pitch)
            self.setPos(self.position)
            self.setRotation(self.angle)
        if view=='z':
            self.position.setX(terminal.x*1000)
            self.position.setY(int(-terminal.y*1000))
            self.angle = -np.rad2deg(terminal.yaw)
            self.setPos(self.position)
            self.setRotation(self.angle)
    def UpdateTargetPos(self,entity:geometric.Terminal,view='x'):
        '''
        获取当前坐标
        :param view:
        :return:
        '''
        if view=='x':
            entity.target.y=self.position.x()*0.001
            entity.target.z=self.position.y()*-0.001
            entity.target.roll=-np.deg2rad(self.angle)
        if view=='y':
            entity.target.x=self.position.x()*0.001
            entity.target.z=self.position.y()*-0.001
            entity.target.pitch=-np.deg2rad(self.angle)
        if view=='z':
            entity.target.x = self.position.x()*0.001
            entity.target.y = self.position.y()*-0.001
            entity.target.yaw = -np.deg2rad(self.angle)

    def boundingRect(self):
        '''
        获取外框
        :return:
        '''
        return QRectF(-self.sizeX/2, -self.sizeY/2, self.sizeX, self.sizeY)

    def paint(self, painter,option, widget):
        '''
        绘制
        :param painter:
        :return:
        '''
        painter.fillRect(self.boundingRect(),self.color)
        painter.drawText(self.boundingRect(),Qt.AlignCenter,self.name)
    def mouseMoveEvent(self, event):
        self.parent.focused = True
        if event.buttons() == Qt.LeftButton:
            delta = event.pos() - event.lastPos()
            self.position += delta
            self.setPos(self.position)
        else:
            delta = event.pos() - event.lastPos()
            self.angle -= delta.x()
            self.setRotation(self.angle)
    def mouseReleaseEvent(self, event):
        self.parent.focused=False

class Link2D(QGraphicsObject):
    '''
    关节
    '''
    def __init__(self,startX=0,startY=0,endX=0,endY=0,color=Qt.blue,width=4):
        super().__init__()
        self.start=QPointF(startX,startY)
        self.end=QPointF(endX,endY)
        self.rot=0.0
        self.color=color
        self.width=width
        #self.setFlag(QGraphicsItem.ItemIsMovable)
    def paint(self, painter, option, widget):
        pen = QPen(self.color, self.width, Qt.SolidLine)
        painter.setPen(pen)
        painter.drawLine(self.start, self.end)
        #painter.drawEllipse(self.start,2,2)
        #painter.drawText(self.start,"{:.3f}".format(self.rot))

    def boundingRect(self):
        # 返回图形项的边界矩形
        sizeX=np.abs(self.start.x()-self.end.x())+self.width
        sizeY = np.abs(self.start.y() - self.end.y())+self.width
        x=0
        y=0
        if self.start.x()>self.end.x():
            x=self.end.x()-self.width
        else:
            x=self.start.x()-self.width
        if self.start.y()>self.end.y():
            y=self.end.y()-self.width
        else:
            y=self.start.y()-self.width
        return QRectF(x,y,sizeX,sizeY)

    def UpdatePos(self,start:geometric.Terminal,end:geometric.Terminal,view='z'):
        '''
        :param start:
        :param end:
        :param view:
        :return:
        '''
        self.prepareGeometryChange()
        if view=='x':
            self.start.setX(start.y*1000)
            self.start.setY(-start.z*1000)
            self.end.setX(end.y*1000)
            self.end.setY(-end.z*1000)
            self.rot=start.roll
        if view=='y':
            self.start.setX(start.x * 1000)
            self.start.setY(-start.z * 1000)
            self.end.setX(end.x*1000)
            self.end.setY(-end.z * 1000)
            self.rot=start.pitch
        if view=='z':
            self.start.setX(start.x * 1000)
            self.start.setY(-start.y * 1000)
            self.end.setX(end.x*1000)
            self.end.setY(-end.y * 1000)
            self.rot=start.yaw
        #self.update()


class RobotModel2D(QGraphicsView):
    def __init__(self,name='顶视图',xRange=[-0.18,0.18],yRange=[-0,0.45],view='x'):
        super().__init__()
        self.view=view
        self.focused=False
        self.name=name
        self.scene = QGraphicsScene(self)
        self.InitScene(xRange,yRange)
        self.terminalList=[]
        self.chainList=[]

    def AddChain(self,chain:geometric.AppleChain,color=Qt.yellow):
        '''添加Chain'''
        bones=[]
        for i in chain.chain:
            l=Link2D(i.start.y,i.start.y,i.end.x,i.end.z,color=color)
            self.scene.addItem(l)
            bones.append(l)
        self.chainList.append(bones)
    def AddTerminal(self,terminalEntity:geometric.Terminal):
        '''
        添加一个Terminal
        :param size:
        :param position:
        :param name:
        :return:
        '''
        terminal=None
        if self.view=='x':
            terminal=Terminal2D(parent=self,sizeX=terminalEntity.size[1],sizeY=terminalEntity.size[2],
                                posX=terminalEntity.y,posY=terminalEntity.z,name=terminalEntity.name)
        if self.view=='y':
            terminal=Terminal2D(parent=self,sizeX=terminalEntity.size[0],sizeY=terminalEntity.size[2],
                                posX=terminalEntity.x,posY=terminalEntity.z,name=terminalEntity.name)
        if self.view=='z':
            terminal = Terminal2D(parent=self, sizeX=terminalEntity.size[0], sizeY=terminalEntity.size[1],
                                  posX=terminalEntity.x, posY=terminalEntity.y, name=terminalEntity.name)
        self.terminalList.append(terminal)
        self.scene.addItem(terminal)

    def UpdateChain(self,id,chain:geometric.AppleChain):
        '''
        更新Chain
        :param id:
        :param chain:
        :return:
        '''

        bone=chain.chain
        for i in range(len(bone)):
            self.chainList[id][i].UpdatePos(bone[i].start,bone[i].end,view=self.view)




    def InitScene(self,xRange=[-0.18,0.18],yRange=[-0,0.45]):
        '''
        初始化场景
        :param xRange:
        :param yRange:
        :return:
        '''
        x0=int(xRange[0]*1000)
        x1=int(xRange[1]*1000)
        y0=int(yRange[0]*1000)
        y1=int(yRange[1]*1000)
        self.setBackgroundBrush(Qt.lightGray)
        self.setScene(self.scene)
        axis_x = QGraphicsLineItem(xRange[0]*1000, 0, xRange[1]*1000, 0)  # Assuming each pixel represents 0.001m
        self.scene.addItem(axis_x)
        # Draw tick marks and labels on the horizontal axis
        for i in range(x0, x1+1, 50):
            tick = QGraphicsLineItem(i, -5, i, 5)
            self.scene.addItem(tick)
            label = QGraphicsTextItem(f'{i / 1000:.2f}m')
            label.setPos(i - 15, 10)
            self.scene.addItem(label)
        # Draw vertical axis
        axis_y = QGraphicsLineItem(0, -y0, 0, -y1)
        self.scene.addItem(axis_y)
        # Draw tick marks and labels on the vertical axis
        for i in range(-y0, -y1-1, -49):
            tick = QGraphicsLineItem(-5, i, 5, i)
            self.scene.addItem(tick)
            label = QGraphicsTextItem(f'{-i / 1000:.2f}m')
            label.setPos(-30, i - 5)
            self.scene.addItem(label)
        self.setScene(self.scene)




if __name__ == '__main__':
    app = QApplication(sys.argv)
    view = RobotModel2D()
    view.AddTerminal(geometric.Terminal(0,0,0))
    view.update()
    view.show()
    sys.exit(app.exec_())
